/**
 * Project:   dtu.fmes.googleloon
 * File:      FindTargetHeightSimulationLink.java
 * License: 
 *            This file is licensed under GNU General Public License version 3
 *            http://www.gnu.org/licenses/gpl-3.0.txt
 *
 * Copyright:	Nicolai Sonne [ NicolaiSonne@hotmail.com.com ]
 * Date:      Dec 11, 2013
 */

package dtu.fmes.loon.simulation.processors;

import java.util.ArrayList;
import java.util.List;

import dtu.fmes.loon.model.Balloon;
import dtu.fmes.loon.model.Vector;
import dtu.fmes.loon.model.environment.Wind;
import dtu.fmes.loon.utils.VectorUtils;

/**
 * This simulation link simulates find the desired altitude for balloons by
 * finding their target position. Target position is found by measuring and
 * optimizing distance to neighboring balloons.
 * 
 * @author Nicolai Sonne
 * 
 */
public class FindTargetHeightSimulationLink extends SimulationLink {

	private Double xPosMax = null;
	private Double yPosMax = null;
	private Double indent = null;

	/**
	 * Finds all alive balloons, which are within the communication of the
	 * original balloon
	 * 
	 * @param balloon
	 * @return
	 */
	private List<Balloon> getBalloonsInRange(Balloon balloon) {
		List<Balloon> balloonsInRange = new ArrayList<Balloon>();

		for (Balloon otherBalloon : css.getBalloons()) {
			if (otherBalloon.isAlive()) {
				if (!balloon.getId().equals(otherBalloon.getId())) {
					Double latitudeDiff = balloon.getLatitude() - otherBalloon.getLatitude();
					Double longitudeDiff = balloon.getLongitude() - otherBalloon.getLongitude();

					if (Math.sqrt(Math.pow(latitudeDiff, 2) + Math.pow(longitudeDiff, 2)) <= cbv.getCommunication().getRange()) {
						balloonsInRange.add(otherBalloon);
					}
				}
			}
		}

		return balloonsInRange;
	}

	/**
	 * Finds an optimal position of balloon (from a neighboring balloon). This
	 * is done by contracting/extending the vector between itself and its
	 * neighbor balloon.
	 * 
	 * @param main
	 * @param other
	 * @return
	 */
	private Vector getOptimalPosition(Balloon main, Balloon other) {
		// Move connecting vector to origo.
		Vector direction = VectorUtils.moveToOrigo(new Vector(other.getLatitude(), other.getLongitude()), new Vector(main.getLatitude(), main.getLongitude()));

		// Find multiplier of vector to achieve optimal range
		Double multiplier = cbv.getCommunication().getOptimalRange() / Math.sqrt(Math.pow(direction.getX(), 2) + Math.pow(direction.getY(), 2));

		// Calculate desired lat/long
		Double optimalLatitude = other.getLatitude() + direction.getX() * multiplier;
		Double optimalLongitude = other.getLongitude() + direction.getY() * multiplier;

		return new Vector(optimalLatitude, optimalLongitude);
	}

	/**
	 * Find optimal altitude to go from one position to another
	 * 
	 * @param originalPosition
	 * @param optimalPosition
	 * @return
	 */
	private Double getOptimalAltitude(Vector originalPosition, Vector optimalPosition) {
		// Gets corresponding wind stack
		Wind[] wind = css.getEnvironment().getWindArray(originalPosition);

		// Generate connecting vector and moves to origo
		Vector directionVector = VectorUtils.moveToOrigo(originalPosition, optimalPosition);

		double difference = 9999999999999.0;
		double optimalAltitude = 0.0;

		// Find wind from wind stack that differs the least from desired
		// direction.
		for (int i = 0; i < wind.length; i++) {
			Wind currentWind = wind[i];
			Double currentDifference = VectorUtils.getAngleBetweenVectors(directionVector, currentWind.getVector());
			if (currentDifference < difference) {
				difference = currentDifference;
				optimalAltitude = currentWind.getHeight();
			}
		}

		// Return altitude of found wind
		return optimalAltitude;
	}

	@Override
	public void run() {
		// Calculate padding of area
		indent = Math.sqrt(Math.pow(css.getConstantBalloonValues().getCommunication().getOptimalRange(), 2) / 2);
		xPosMax = css.getEnvironment().getArea().getxSize() - indent;
		yPosMax = css.getEnvironment().getArea().getySize() - indent;

		Vector optimalPosition = null;

		Long currentTime = css.getSimulation().getCurrentTime();
		Double frequency = css.getConstantBalloonValues().getCommunication().getFrequency();

		for (Balloon balloon : css.getBalloons()) {
			if (balloon.isAlive()) {
				// Check if frequency allows the balloon to be updated
				if (balloon.getLastUpdated() == null || (currentTime - balloon.getLastUpdated()) >= frequency) {
					balloon.setOptimalPos(new ArrayList<Vector>());

					// Get all alive balloons in range
					List<Balloon> balloonsInRange = getBalloonsInRange(balloon);

					if (!balloonsInRange.isEmpty()) {
						optimalPosition = new Vector(0.0, 0.0);

						// Get optimal positions of all neighbors in range
						for (Balloon balloonInRange : balloonsInRange) {
							Vector oPos = getOptimalPosition(balloon, balloonInRange);
							balloon.getOptimalPos().add(oPos);
							optimalPosition.add(oPos);
						}

						// Calculate average of all optimal position
						optimalPosition.divide((double) balloonsInRange.size());
					} else {
						// If no alive neighboring balloons are in range, try to
						// stay in same position
						optimalPosition = new Vector(balloon.getLatitude(), balloon.getLongitude());
					}

					// Set update time for frequency constraints
					balloon.setLastUpdated(currentTime);

					// Set desired position
					balloon.setTargetPosition(fixPosition(optimalPosition));

					// Find suiting altitude of desired position
					balloon.setTargetAltitude(getOptimalAltitude(new Vector(balloon.getLatitude(), balloon.getLongitude()), balloon.getTargetPosition()));
				}
			}
		}
	}

	// If position is outside area move it into area.
	// Will only correct one direction: the one which is most away from area
	public Vector fixPosition(Vector position) {

		Double x = position.getX();
		Double y = position.getY();

		Direction d = Direction.GOOD;
		Double distance = 0.0;

		if (x > xPosMax) {
			Double newDistance = x - xPosMax;
			if (worse(distance, newDistance)) {
				distance = newDistance;
				d = Direction.LEFT;
			}
		}

		if (x < indent) {
			Double newDistance = Math.abs(x - indent);
			if (worse(distance, newDistance)) {
				distance = newDistance;
				d = Direction.RIGHT;
			}
		}

		if (y > yPosMax) {
			Double newDistance = y - yPosMax;
			if (worse(distance, newDistance)) {
				distance = newDistance;
				d = Direction.DOWN;
			}
		}

		if (y < indent) {
			Double newDistance = Math.abs(y - indent);
			if (worse(distance, newDistance)) {
				distance = newDistance;
				d = Direction.UP;
			}
		}

		switch (d) {
			case UP:
				y = indent;
				break;
			case DOWN:
				y = yPosMax;
				break;
			case RIGHT:
				x = indent;
				break;
			case LEFT:
				x = xPosMax;
				break;
			default:
				break;
		}

		return new Vector(x, y);
	}

	private boolean worse(Double orgDistance, Double newDistance) {
		if (newDistance > orgDistance) return true;
		return false;
	}

	private enum Direction {
		GOOD, UP, DOWN, RIGHT, LEFT;
	}
}
